//
// Created by 26090 on 24-12-25.
//

#include "arm_cmd.h"
#include "bsp_usart.h"
#include <bsp_log.h>
#include <cmsis_os2.h>

#include "message.h"
#include "message_center.h"
#include "mastermachine.h"
#include "command.h"
#include "robot.h"


#define CAMERA_OFF 95
#define CAMERA_HALF 150
#define CAMERA_ON 180

static Publisher_t *arm_cmd_pub;
// static Subscriber_t *arm_feed_sub;

static Arm_Ctrl_Cmd_s arm_cmd_send;

void action_set_angle(float yaw,float height,float length,float open);
void action_set_camera(float camera_angle);

static Master_recv_s *master_recv;

static bool is_raw_cali=false;

void arm_debug_wait()
{
    static bool arm_debug_continue=false;
    while(!arm_debug_continue)
    {
        osDelay(5);
    }
    arm_debug_continue=false;

}

//全局初始化函数
void action_init(void)
{
    arm_cmd_pub = PubRegister("arm_cmd", sizeof(Arm_Ctrl_Cmd_s));
    // action_set_angle(-26,22,23,1);
    // action_set_angle(-26,28,15,1);
    action_set_angle(0,18.5,14,1);
    osDelay(1000);
    master_recv=CommunicateInit(&huart1);
    action_set_camera(CAMERA_OFF);
}

void action_calibrate(State_e state)
{
    switch (state)
    {
    case STATE_IDLE:
        action_set_angle(0,18.5,13,1);
        break;
    case STATE_PICK_RAW:
        action_set_angle(180,18.5,13,1);
        action_set_camera(CAMERA_ON);
        break;
    case STATE_ROUGH_AREA:
    case STATE_FASHIONING_AREA:
        action_set_angle(180,18.5,23.2,1);
        osDelay(1000);
        action_set_angle(180,10,23.2,1);
        action_set_camera(CAMERA_HALF);
        break;
    case STATE_FINISHED:

    default:
        break;
    }
    osDelay(2500);
}

void put_plate(uint16_t plate_number);
void take_plate(uint16_t plate_number);

/*********************** 选颜色放置环节 ***************************/
// 车体放置位置选盘
void select_plate(uint16_t plate_number,float *yaw,float* length)
{
    switch(plate_number)
    {
    case COLOR_RED:
        *yaw=23.4;
        *length=19.1;
        break;
    case COLOR_GREEN:
        *yaw=1;
        *length=18.0;
        break;
    case COLOR_BLUE:
        *yaw=-20.0;
        *length=19.1;
        break;
    default:
        while(1)
        {
            LOGERROR("Plate number is invalid");
            osDelay(100);
        }

    }

}

void select_ring(uint16_t ring_number,float *yaw,float* length)
{
    switch(ring_number)
    {
    case COLOR_RED:
        *yaw=148.5;
        *length=27.8;
        break;
    case COLOR_GREEN:
        *yaw=181;
        *length=23.0;
        break;
    case COLOR_BLUE:
        *yaw=215;
        *length=27.8;
        break;
    default:
        while(1)
        {
            LOGERROR("Plate number is invalid");
            osDelay(100);
        }

    }
}

//原料区
void action_pick_raw(uint16_t task)
{
    //两种状态：已知正反转和未知正反转
    //已知正反转可以有预测机制，位置正反转不能预测。
    for(int i = 100 ; i > 0 ;i/=10)
    {
        Color_e color = task/i%10;
        uint8_t pick_target_pos=0;
        while(1)
        {
            //第一步分支：是否已停止
            if(master_recv->is_stop == 1)
            {

                //等待颜色正确,且剩余时间够夹
                if(master_recv->remain_time >=2.0f)
                {
                    if(master_recv->color[CURRENT][2]==color )
                    {
                        pick_target_pos=2;
                        break;
                    }
                    //如果只抓前面，则注释下面代码。
                    if( master_recv->color[CURRENT][0]==color)
                    {
                        pick_target_pos=0;
                        break;
                    }
                    if(i==1 && master_recv->color[CURRENT][1]==color)
                    {
                        pick_target_pos=1;
                        break;
                    }
                }
            }
            else
            {
                //未停止
                //只有在已知旋转方向，且FUTURE有时再进行抓取
                if(master_recv->direction!=DIRECTION_UNKNOWN)
                {
                    // action_set_angle(120,18.5,22,1);
                    //等待颜色正确,且剩余时间够夹
                    if(master_recv->color[FUTURE][2]==color )
                    {
                        pick_target_pos=2;
                        break;
                    }
                    if( master_recv->color[FUTURE][0]==color)
                    {
                        pick_target_pos=0;
                        break;
                    }
                    if( i == 1 && master_recv->color[FUTURE][1]==color)
                    {
                        pick_target_pos = 1;
                        break;
                    }
                }

            }

            osDelay(5);
        }

        float yaw_target,length_target;
        uint16_t delay_time=0;
        //右上方位置
        switch(pick_target_pos)
        {
        // case 0:
        //     // yaw_target=162.4f;
        //     yaw_target=169.0f;
        //     length_target=28.5f;
        //     delay_time=100;
        //     break;
        // case 1:
        //     yaw_target=218.0f;
        //     length_target=28.50f;
        //     delay_time=500;
        //     break;
        // case 2:
        //     yaw_target=180.0f;
        //     length_target=14.0f;
        //     delay_time=300;
        //     break;
        // default:
        //     LOGERROR("pick_target_pos is invalid");
        //     break;

        //全居中代码
        case 0:
        yaw_target=162.4f;
        length_target=28.5f;
        delay_time=100;
        break;
        case 1:
            yaw_target=198.0f;
        length_target=28.50f;
        delay_time=500;
        break;
        case 2:
            yaw_target=180.0f;
        length_target=14.0f;
        delay_time=300;
        break;

        //偏右代码
        // case 0:
        //     // yaw_target=162.4f;
        //     yaw_target=169.0f;
        //     length_target=28.5f;
        //     delay_time=100;
        //     break;
        // case 1:
        //     yaw_target=218.0f;
        //     length_target=28.50f;
        //     delay_time=500;
        //     break;
        // case 2:
        //     yaw_target=180.0f;
        //     length_target=14.0f;
        //     delay_time=300;
        //     break;
        // default:
        //     LOGERROR("pick_target_pos is invalid");
        //     break;
        }

        action_set_angle(yaw_target,18,length_target,1);
        if(pick_target_pos==1)
            action_set_camera(CAMERA_HALF);
        osDelay(601+delay_time);
        // debug_wait();

        while (master_recv->is_stop==0)
        {
            osDelay(2);
        }

        action_set_angle(yaw_target,10.5,length_target,1);
        osDelay(300);

        action_set_angle(yaw_target,10.5,length_target-3,0);
        osDelay(300);

        action_set_camera(CAMERA_ON);

        put_plate(color);

    }


}



//放置到粗加工区
void action_put_rough(uint16_t task)
{
    // action_set_camera(25);
    float yaw_target,length_target;
    // 放置三种颜色
    for(int i = 100 ; i > 0 ;i/=10)
    {
        Color_e color = task/i%10;
        select_ring(color,&yaw_target,&length_target);

        //开始执行
        //从盘中取出
        take_plate(color);

        action_set_angle(yaw_target,18.5,12.0,0);
        osDelay(700+color*120+300);
        //前伸并下降
        action_set_angle(yaw_target,10.0,length_target,0);
        osDelay(400);
        //放置
        action_set_angle(yaw_target,4.5,length_target,0);
        osDelay(400);
        //松开
        action_set_angle(yaw_target,4.5,length_target,1);
        osDelay(300);
        //抬起
        action_set_angle(yaw_target,18.5,length_target,1);
        osDelay(400);
    }


}


//从粗加工区拿回物料
void action_take_rough(uint16_t task)
{

    float yaw_target,length_target;
    // 三种颜色
    for(int i = 100 ; i > 0 ;i/=10)
    {
        Color_e color = task/i%10;
        select_ring(color,&yaw_target,&length_target);

        //开始执行
        //

        action_set_angle(yaw_target,18.5,length_target,1);
        osDelay(400+color*200);
        // debug_wait();

        //下去
        action_set_angle(yaw_target,4.5,length_target,1);
        osDelay(400);
        // debug_wait();
        //获取
        action_set_angle(yaw_target,4.5,length_target,0);
        osDelay(350);


        action_set_angle(yaw_target,18.5,length_target,0);
        osDelay(400);

        put_plate(color);



    }


}

//放置到精加工区
void action_put_fashioning(uint16_t task)
{
    // action_set_camera(25);
    float yaw_target,length_target;
    // 放置三种颜色
    for(int i = 100 ; i > 0 ;i/=10)
    {
        Color_e color = task/i%10;
        select_ring(color,&yaw_target,&length_target);

        //开始执行
        //从盘中取出
        take_plate(color);


        action_set_angle(yaw_target,18.5,12.0,0);
        osDelay(700+color*120+300);
        //放置
        action_set_angle(yaw_target,11.5,length_target,0);
        osDelay(400);
        //松开
        action_set_angle(yaw_target,11.5,length_target,1);
        osDelay(300);
        //抬起
        action_set_angle(yaw_target,18.5,length_target,1);
        osDelay(400);
    }


}


void put_plate(uint16_t plate_number)
{
    float yaw,length;

    select_plate(plate_number,&yaw,&length);

    //转回去
    action_set_angle(yaw,18.5,26,0);
    osDelay(650+plate_number*150);

    //缩短
    action_set_angle(yaw,18.5,length,0);
    osDelay(300);

    // debug_wait();
    //下去
    action_set_angle(yaw,14,length,0);
    osDelay(300);

    // debug_wait();

    //打开夹爪，并略微抬高
    action_set_angle(yaw,14,length,1);
    osDelay(250);

    //抬起来
    action_set_angle(yaw,18.5,18,1);
    osDelay(500);
}

//耗时约为3.9s
void take_plate(uint16_t plate_number)
{
    float yaw,length;

    select_plate(plate_number,&yaw,&length);

    //转到盘位置
    action_set_angle(yaw,18.5,length,1);
    osDelay(500+plate_number*150);
    //下去抓取
    action_set_angle(yaw,13,length,1);
    osDelay(400);

    //抱住
    action_set_angle(yaw,13,length,0);
    osDelay(300);

    //上去
    action_set_angle(yaw,18.5,length,0);
    osDelay(600);

    //缩后方
    action_set_angle(yaw,18.5,12.0,0);
    osDelay(400);

    // //转过去前面
    // action_set_angle(180,22,26.5,0);
    // osDelay(1400);
}

void action_set_angle(float yaw,float height,float length,float open)
{
    arm_cmd_send.yaw=yaw;
    arm_cmd_send.height=height;
    arm_cmd_send.length=length;
    arm_cmd_send.open_angle=open;

    // arm_cmd_send.update_flag |= 0x0F;
    PubPushMessage(arm_cmd_pub,&arm_cmd_send);
    // arm_cmd_send.update_flag = 0;
}

void action_set_camera(float camera_angle)
{
    arm_cmd_send.camera_angle=camera_angle;
    // arm_cmd_send.update_flag |=0x10;
    PubPushMessage(arm_cmd_pub,&arm_cmd_send);
    // arm_cmd_send.update_flag = 0;
}

